#include "gero.h"

#define PERROR(s) {perror(s); exit(1);}

extern spe_program_handle_t fmaf4_spu;
extern spe_program_handle_t hello1_spu;

/********************************************************************************/
/*************************** Fonctions pour ocaml *******************************/  
/********************************************************************************/

void *ppu_pthread_function(void *arg) {
  ppu_pthread_data_t *datap = (ppu_pthread_data_t *)arg;
  if (spe_context_run(datap->spe_ctx, &datap->entry, 0,datap->argp  , NULL, NULL) < 0) 
    PERROR ("Failed running context");
  pthread_exit(NULL);
}

value gero_pthread_create(value ctx)
{
  CAMLparam1(ctx);
  CAMLlocal1(r);
  pthread_t thread;
  if (pthread_create (&thread, NULL, &ppu_pthread_function, &ctx))  
    PERROR ("creation thread");
  r = (value) thread;
  CAMLreturn(r);
}

value gero_join(value t)
{
  CAMLparam1(t);
  if (pthread_join ((pthread_t)t, NULL))                                                                                      
    PERROR("probleme pthread_join");
  CAMLreturn(Val_unit); 
}

value gero_spe_context_destroy(value v)
{
  CAMLparam1(v);
  /* CONTEXT Destruction  */
  if (spe_context_destroy ((spe_context_ptr_t)v) != 0) 
    PERROR("probleme destroying context");
  printf("thread ending C\n");
  CAMLreturn(Val_unit);
}

value  gero_spe_count_usable_spes(void)
{
  return Val_int(spe_cpu_info_get(SPE_COUNT_USABLE_SPES, -1));
}

value  gero_spe_count_physical_spes(void)
{
  return Val_int(spe_cpu_info_get(SPE_COUNT_PHYSICAL_SPES, -1));
}

value gero_spe_context_create(void)
{
  CAMLparam0();
  CAMLlocal1(r);
  spe_context_ptr_t ctx;
  if ((ctx = spe_context_create (0, NULL)) == NULL)
    PERROR ("creation context");
  r = (value) ctx;
  CAMLreturn(r);
}


value gero_spe_context_run(value ctx)
{
  CAMLparam1(ctx);
  uint32_t entry = SPE_DEFAULT_ENTRY;
  if (spe_context_run((spe_context_ptr_t)ctx, &entry, 0, NULL, NULL, NULL) < 0)
    PERROR ("Failed running context"); 
  CAMLreturn(Val_unit); 
}

value gero_spe_image_open(value spe_prog)
{
  CAMLparam1(spe_prog);
  CAMLlocal1(r);
  spe_program_handle_t* p;
  p = spe_image_open(String_val(spe_prog));
  r = (value) p;
  CAMLreturn(r);
}

/************ Maths **********/
void  FMAF4_C  (GERO_ARGUMENTS *arg)
{
  int nb_iter;
  long i;
  /*
    register vector float vec1;
    register vector float vec2;
    register vector float vec3;
  */
  
  nb_iter = arg->size/4;
  
  for (i=0; i<nb_iter; i++)
    {
      /*
	vec1 = vec_ld(4*i*sizeof(float), (float*) arg->vector1);
	vec2 = vec_ld(4*i*sizeof(float), (float*) arg->vector2);
	vec3 = vec_ld(4*i*sizeof(float), (float*) arg->vector3);
	*((vector float*) (arg->result)+i)  = fmaf4(vec1,vec2, vec3 );
	*/

    *((vector float*) (arg->result)+i)  = vec_madd(
    					*((vector float*)(arg->vector1)+i), 
						  *((vector float*)(arg->vector2)+i), 
						  *((vector float*)(arg->vector3)+i));
    }
}

value gero_fmaf4(value x, value y, value z)
{

  CAMLparam3(x,y,z);
  CAMLlocal1(output);
  int  type;
  long size;
  GERO_ARGUMENTS arg;
  arg.vector1 = Data_bigarray_val(x);
  arg.vector2 = Data_bigarray_val(y);
  arg.vector3 = Data_bigarray_val(z);

  type = Bigarray_val(x)->flags & BIGARRAY_KIND_MASK;

  arg.size = Bigarray_val(x)->dim[0];
  
  ALLOC_RESULT_ARRAY(arg.size, float);        
  //arg.result= (void*)memalign(16,arg.size*sizeof(float));
  FMAF4_C (&arg);

  output =  alloc_bigarray_dims( type| BIGARRAY_C_LAYOUT, 1, arg.result, arg.size); 
  //free(arg.result);
  CAMLreturn(output);
}

value gero_load_fmaf4_spu(value ctx)
{
  CAMLparam1(ctx);
  if(spe_program_load((spe_context_ptr_t)ctx, &fmaf4_spu) != 0)
    {
      fprintf(stderr, "Error, Failed spe_program_load (errno=%d)\n", errno);
      exit(1);
    }
  CAMLreturn(Val_unit);
}

inline uint32 MIN(uint32 a, uint32 b) {
  return a < b ? a : b;
}

value gero_fmaf4_spu(value x, value y, value z, value ctx)
{

  CAMLparam4(x,y,z, ctx);
  CAMLlocal1(output);
  GERO_ARGUMENTS args[6] __attribute__ ((aligned(128))); 
  CB_4V cbs[6] __attribute((aligned(128))); 
  ppu_pthread_data_t datas[6] __attribute__ ((aligned(128)));
  int type = Bigarray_val(x)->flags & BIGARRAY_KIND_MASK;
  int i = 0;
  long size = Bigarray_val(x)->dim[0];
  long nb_vec = size/4;
  long q = nb_vec/6;
  long r = nb_vec%6;
  long opti = q*4*sizeof(float);
  float* res_final;
  res_final = (float*)memalign(16, (size*sizeof(float)& ~63 ) + 64 );
  //  printf("PPE opti: %ld\n", opti);
  
  // On partage le calcul entre les 6 SPEs
  if (q > 0)
    {
      for (i=0; i<MIN(5,spe_cpu_info_get(SPE_COUNT_USABLE_SPES, -1)-1); i++) //les 5 premiers SPE effectuent le calculs pour q vecteurs de 128b
  	{
	  args[i].size = q;
	  args[i].vector1 = Data_bigarray_val(x)+(i*opti); // ATTENTION OPTI PTET TROP GROS A DIVISER PAR sizeof(float)
	  args[i].vector2 = Data_bigarray_val(y)+(i*opti);
	  args[i].vector3 = Data_bigarray_val(z)+(i*opti);
	  //args[i].result = (void*)memalign( 16, (opti  & ~63 ) + 64 );
	  datas[i].spe_ctx = (spe_context_ptr_t)spe_context_create(0,NULL);
	  datas[i].entry = SPE_DEFAULT_ENTRY;
	  spe_program_load((spe_context_ptr_t)datas[i].spe_ctx, &fmaf4_spu);
	  //printf( "vec1-%d: %f %f %f %f\n",i, *((float*)args[i].vector1), *((float*)args[i].vector1+1), *((float *)args[i].vector1+2), *((float *)args[i].vector1+3));
	  //printf( "vec2-%d: %f %f %f %f\n",i, *((float*)args[i].vector2), *((float*)args[i].vector2+1), *((float *)args[i].vector2+2), *((float *)args[i].vector2+3));
	  //printf( "vec3-%d: %f %f %f %f\n",i, *((float*)args[i].vector3), *((float*)args[i].vector3+1), *((float *)args[i].vector3+2), *((float *)args[i].vector3+3));
	  cbs[i].vec1_addr = (uint64 )((uintptr_t)args[i].vector1);
	  cbs[i].vec2_addr = (uint64 )((uintptr_t)args[i].vector2);
	  cbs[i].vec3_addr = (uint64 )((uintptr_t)args[i].vector3);
	  cbs[i].res_addr = (uint64 )((uintptr_t)(res_final+(i*opti/sizeof(float))));
	  cbs[i].size = opti/4; 
	  datas[i].argp = (uint64 )(uintptr_t)(cbs+i);	
	 /* printf ("PPE cb: %lld\n", (uint64 )(uintptr_t)&cbs[i]); fflush(stdout);
	    printf ("PPE vec1: %lld\n", (uint64 )(uintptr_t)cbs[i].vec1_addr); fflush(stdout);
	    printf ("PPE vec2: %lld\n", (uint64 )(uintptr_t)cbs[i].vec2_addr); fflush(stdout);
	    printf ("PPE vec3: %lld\n", (uint64 )(uintptr_t)cbs[i].vec3_addr); fflush(stdout);
	    printf ("PPE res: %lld\n", (uint64 )(uintptr_t)cbs[i].res_addr); fflush(stdout);
   		*/
	  if (pthread_create (&datas[i].pthread, NULL,&ppu_pthread_function, &datas[i])) 
	    {
	      perror("Failed creating thread");
	      exit(1);
	    }
	}
    }
  
  long opti2 = (q+r)*4*sizeof(float);
  args[i].size = q+r;
  args[i].vector1 = Data_bigarray_val(x)+(i*opti);
  args[i].vector2 = Data_bigarray_val(y)+(i*opti);
  args[i].vector3 = Data_bigarray_val(z)+(i*opti);
  //args[i].result = (float*)memalign( 16, (opti2  & ~63 ) + 64 );
  datas[i].spe_ctx = (spe_context_ptr_t)spe_context_create(0,NULL);
  datas[i].entry = SPE_DEFAULT_ENTRY;
  spe_program_load((spe_context_ptr_t)datas[i].spe_ctx, &fmaf4_spu);
  //printf( "vec1-%d: %f %f %f %f\n",i, *((float*)args[i].vector1), *((float*)args[i].vector1+1), *((float *)args[i].vector1+2), *((float *)args[i].vector1+3));
  //printf( "vec2-%d: %f %f %f %f\n",i, *((float*)args[i].vector2), *((float*)args[i].vector2+1), *((float *)args[i].vector2+2), *((float *)args[i].vector2+3));
  //printf( "vec3-%d: %f %f %f %f\n",i, *((float*)args[i].vector3), *((float*)args[i].vector3+1), *((float *)args[i].vector3+2), *((float *)args[i].vector3+3));
  cbs[i].vec1_addr = (uint64 )((uintptr_t)args[i].vector1);
  cbs[i].vec2_addr = (uint64 )((uintptr_t)args[i].vector2);
  cbs[i].vec3_addr = (uint64 )((uintptr_t)args[i].vector3);
  //cbs[i].res_addr = (uint64 )((uintptr_t)args[i].result);	
  cbs[i].res_addr = (uint64 )((uintptr_t)(res_final+(i*opti/sizeof(float))));
  //printf( "2 vec1-%d: %f %f %f %f\n",i, *((float*)cbs[i].vec1_addr), *((float*)cbs[i].vec1_addr+1), *((float *)cbs[i].vec1_addr+2), *((float *)cbs[i].vec1_addr+3));
  cbs[i].size = opti2/4; 
  datas[i].argp =  (uint64 )(uintptr_t)(cbs+i);	
		/*	 printf ("PPE cb: %lld\n", (uint64 )(uintptr_t)&cbs[i]); fflush(stdout);
	    printf ("PPE vec1: %lld\n", (uint64 )(uintptr_t)cbs[i].vec1_addr); fflush(stdout);
	    printf ("PPE vec2: %lld\n", (uint64 )(uintptr_t)cbs[i].vec2_addr); fflush(stdout);
	    printf ("PPE vec3: %lld\n", (uint64 )(uintptr_t)cbs[i].vec3_addr); fflush(stdout);
	    printf ("PPE res: %lld\n", (uint64 )(uintptr_t)cbs[i].res_addr); fflush(stdout);*/
  if (pthread_create (&datas[i].pthread, NULL,&ppu_pthread_function, &datas[i])) 
    {
      perror("Failed creating thread");
      exit(1);
    }
  // printf("NB SPE LANCES: %d\n", i+1);
  int j;
  for (j=0; j<i;j++)
    {
      pthread_join(datas[j].pthread, NULL);
    }

	
  output =  alloc_bigarray_dims( type | BIGARRAY_C_LAYOUT, 1,  res_final, size);
  CAMLreturn(output);
}


/********************************************************************************/

value gero_load_hello1_spu(value ctx) {
  CAMLparam1(ctx);
  if(spe_program_load((spe_context_ptr_t)ctx, &hello1_spu) != 0)
    {
      fprintf(stderr, "Error, Failed spe_program_load (errno=%d)\n", errno);
      exit(1);
    }
  CAMLreturn(Val_unit);

}
